﻿#include "memath.h"

#include <float.h>

double MeMath::angleBetweenVectors(const double v1[], const double v2[], double axis[3])
{
    double axis_[3];
    MeMath::cross(v1, v2, axis_);
    if (axis) {
        axis[0] = axis_[0];
        axis[1] = axis_[1];
        axis[2] = axis_[2];
    }
    return atan2(MeMath::norm(axis_), MeMath::dot(v1, v2));
}

//----------------------------------------------------------------------------
// Compute distance to finite line. Returns parametric coordinate t
// and point location on line.
double MeMath::distanceToLineSeg(const double x[3], const double p1[3], const double p2[3], double closestPoint[])
{
    double t;
    return distanceToLineSeg(x, p1, p2, t, closestPoint);
}

// Compute distance to finite line. Returns parametric coordinate t
// and point location on line.
double MeMath::distanceToLineSeg(const double x[3], const double p1[3], const double p2[3], double &t, double closestPoint[3])
{
    const double *closest = nullptr;
    // 向量
    double p21[3] = { p2[0]- p1[0], p2[1]- p1[1], p2[2]- p1[2] };

    // 获取最近点的位置
    double num = p21[0]*(x[0]-p1[0]) + p21[1]*(x[1]-p1[1]) + p21[2]*(x[2]-p1[2]);
    if (num == 0.0) {
        closest = p1;
        t = 0;
    }
    else {
        double denom = MeMath::dot(p21,p21);

       // trying to avoid an expensive fabs
       double tolerance = (1e-5)*num;
       if (tolerance < 0.0) {
           tolerance = -tolerance;
       }
       if ( denom < tolerance ) //numerically bad!
       {
           if (num > 0) {
               closest = p2;
               t = DBL_MAX;
           }
           else {
               closest = p1;
               t = -DBL_MAX;
           }
       }
       //
       // If parametric coordinate is within 0<=p<=1, then the point is closest to
       // the line.  Otherwise, it's closest to a point at the end of the line.
       //
       else if ( (t=num/denom) < 0.0 ) {
           closest = p1;
       }
       else if ( t > 1.0 ) {
           closest = p2;
       }
       else {
           closest = p21;
           p21[0] = p1[0] + t*p21[0];
           p21[1] = p1[1] + t*p21[1];
           p21[2] = p1[2] + t*p21[2];
       }
    }

    if (closestPoint)
    {
        closestPoint[0] = closest[0];
        closestPoint[1] = closest[1];
        closestPoint[2] = closest[2];
    }
    return MeMath::distance(closest,x);
}

//----------------------------------------------------------------------------
//
// Determine the distance of the current vertex to the edge defined by
// the vertices provided.  Returns distance. Note: line is assumed
// infinite in extent.
//
double MeMath::distanceToLine(const double x[3], const double p1[3], const double p2[3], double closestPoint[3])
{
    int i;
    double np1[3], p1p2[3], proj, den;

    for (i = 0; i < 3; i++)
    {
        np1[i] = x[i] - p1[i];
        p1p2[i] = p1[i] - p2[i];
    }

    if ((den = MeMath::norm(p1p2)) != 0.0)
    {
        for (i = 0; i < 3; i++)
        {
            p1p2[i] /= den;
        }
    }
    else
    {
        if (closestPoint) {
            closestPoint[0] = p1[0];
            closestPoint[1] = p1[1];
            closestPoint[2] = p1[2];
        }
        return MeMath::distance(x, p1);
    }

    proj = MeMath::dot(np1, p1p2);

    double d2 = (MeMath::dot(np1, np1) - proj * proj);

    if (closestPoint) {
        closestPoint[0] = p1[0] + proj*p1p2[0];
        closestPoint[1] = p1[1] + proj*p1p2[1];
        closestPoint[2] = p1[2] + proj*p1p2[2];
    }

    return sqrt(d2);
}

double MeMath::distanceBetweenLineSegments(double l0[3], double l1[3], // line segment 1
 double m0[3], double m1[3],                                           // line segment 2
 double closestPt1[3], double closestPt2[3],                           // closest points
 double& t1, double& t2)                                               // parametric coords
                                                                       // of the closest points
{
    // Part of this function was adapted from "GeometryAlgorithms.com"
    //
    // Copyright 2001, softSurfer (www.softsurfer.com)
    // This code may be freely used and modified for any purpose
    // providing that this copyright notice is included with it.
    // SoftSurfer makes no warranty for this code, and cannot be held
    // liable for any real or imagined damage resulting from its use.
    // Users of this code must verify correctness for their application.

    const double u[3] = { l1[0] - l0[0], l1[1] - l0[1], l1[2] - l0[2] };
    const double v[3] = { m1[0] - m0[0], m1[1] - m0[1], m1[2] - m0[2] };
    const double w[3] = { l0[0] - m0[0], l0[1] - m0[1], l0[2] - m0[2] };
    const double a = MeMath::dot(u, u);
    const double b = MeMath::dot(u, v);
    const double c = MeMath::dot(v, v); // always >= 0
    const double d = MeMath::dot(u, w);
    const double e = MeMath::dot(v, w);
    const double D = a * c - b * b; // always >= 0
    double sN, sD = D;              // sc = sN / sD, default sD = D >= 0
    double tN, tD = D;              // tc = tN / tD, default tD = D >= 0

    // compute the line parameters of the two closest points

    if (D < 1e-6) {
        // The lines are colinear. Therefore, one of the four endpoints is the
        // point of closest approach
        double minDist = DBL_MAX;
        double* p[4] = { l0, l1, m0, m1 };
        double* a1[4] = { m0, m0, l0, l0 };
        double* a2[4] = { m1, m1, l1, l1 };
        double* uv1[4] = { &t2, &t2, &t1, &t1 };
        double* uv2[4] = { &t1, &t1, &t2, &t2 };
        double* pn1[4] = { closestPt2, closestPt2, closestPt1, closestPt1 };
        double* pn2[4] = { closestPt1, closestPt1, closestPt2, closestPt2 };
        double dist, pn_[3];
        for (unsigned i = 0; i < 4; i++)
        {
            double t = 0;
            dist = MeMath::distanceToLineSeg(p[i], a1[i], a2[i], t, pn_);
            if (dist < minDist)
            {
                minDist = dist;
                *(uv1[i]) = (t < 0. ? 0. : (t > 1. ? 1. : t));
                *(uv2[i]) = static_cast<double>(i % 2); // the corresponding extremum
                for (unsigned j = 0; j < 3; j++)
                {
                    pn1[i][j] = pn_[j];
                    pn2[i][j] = p[i][j];
                }
            }
        }
        return minDist;
    }

    // The lines aren't parallel.

    else { // get the closest points on the infinite lines
        sN = (b * e - c * d);
        tN = (a * e - b * d);
        if (sN < 0.0)
        { // sc < 0 => the s=0 edge is visible
            sN = 0.0;
            tN = e;
            tD = c;
        }
        else if (sN > sD)
        { // sc > 1 => the s=1 edge is visible
            sN = sD;
            tN = e + b;
            tD = c;
        }
    }

    if (tN < 0.0)
    { // tc < 0 => the t=0 edge is visible
        tN = 0.0;

        // recompute sc for this edge
        if (-d < 0.0)
        {
            sN = 0.0;
        }
        else if (-d > a)
        {
            sN = sD;
        }
        else
        {
            sN = -d;
            sD = a;
        }
    }
    else if (tN > tD)
    { // tc > 1 => the t=1 edge is visible
        tN = tD;

        // recompute sc for this edge
        if ((-d + b) < 0.0)
        {
            sN = 0;
        }
        else if ((-d + b) > a)
        {
            sN = sD;
        }
        else
        {
            sN = (-d + b);
            sD = a;
        }
    }

    // finally do the division to get sc and tc
    t1 = (fabs(sN) < 1e-6 ? 0.0 : sN / sD);
    t2 = (fabs(tN) < 1e-6 ? 0.0 : tN / tD);

    // Closest Point on segment1 = S1(t1) = l0 + t1*u
    // Closest Point on segment2 = S1(t2) = m0 + t2*v

    for (unsigned int i = 0; i < 3; i++)
    {
        closestPt1[i] = l0[i] + t1 * u[i];
        closestPt2[i] = m0[i] + t2 * v[i];
    }

    return MeMath::distance(closestPt1, closestPt2);
}


